package behavior;

import Interface.BluetoothNXT;
//import UltrasonicSensor;
import lejos.nxt.*;
import lejos.robotics.navigation.TachoPilot;

import java.io.*;
import java.util.ArrayList;
import java.util.List;
import Interface.TrackMap;

/**
 * atm responsible for constructing an array which holds all the trackPieces. It
 * also makes sure the robot is at a known location before the robot starts
 * analyzing that piece of the track. It assumes that the robot, when done
 * analyzing the current piece, is in the middle of the current piece, facing
 * the exit.
 * 
 */
public class Main {

	private static TachoPilot pilot;
	private static double circ = 5.43; // cm
	private static double axis = 14.95;
	private static final double DEF_CIRC = 5.43;
	private static final double DEF_AXIS = 14.95;
	private static AnalyserController analyserController;
	private static AutoPilot autoPilot;
	private static TrackMap track = new TrackMap();
	//private static ArrayList<Integer> trackList = new ArrayList<Integer>();
	private static SlopeDetector slopeDetector;
	private static BluetoothNXT test;
	private static boolean bt = false;
	private static int adjustIndex = 2;
	private static Panic panic;
	private static boolean bluetooth = false;
	
	
	public static void main(String[] args) throws Exception {
//		System.out.println("Want to calibrate?");
//		int b = Button.waitForPress();
//		if (b == Button.ID_ENTER)
//			calibrate();
		
		//connect to bluetooth
		pilot = new TachoPilot((float) circ, (float) circ, (float) axis, Motor.B, Motor.C,true);

//		analyserController = new AnalyserController(pilot,adjustIndex);
		

//				LCD.drawString("Connect Bluetooth?", 0, 1);
//				int button = Button.waitForPress();
//				if (button == Button.ID_ENTER)
//				{		
//					bluetooth = true; 
//					test = new BluetoothNXT(pilot,new LightSensor(SensorPort.S1), new UltrasonicSensor(SensorPort.S2));
//					test.start();
//					analyserController = new AnalyserController(pilot, adjustIndex, test);
//				}
//				
//				LCD.drawString("Start in 2", 0,0); Thread.sleep(1000); LCD.drawString("Start in 1", 0,1); Thread.sleep(1000);

//				LCD.drawString("Connect Bluetooth?", 0, 1);
//				int button = Button.waitForPress();
//				if (button == Button.ID_ENTER)
//				{		
//					 test = new BluetoothNXT(pilot,new LightSensor(SensorPort.S1), new UltrasonicSensor(SensorPort.S2));
//					test.start();
//					analyserController = new AnalyserController(pilot,test);
//				}
//				
//				LCD.drawString("Start in 2", 0,0); Thread.sleep(1000); LCD.drawString("Start in 1", 0,1); Thread.sleep(1000);
//		
//		

		

		analyserController = new AnalyserController(pilot, adjustIndex);
				
				LCD.drawString("Connect Bluetooth?", 0, 1);
				int button1 = Button.waitForPress();
				if (button1 == Button.ID_ENTER)
				{		
					test = new BluetoothNXT(pilot,new LightSensor(SensorPort.S1), new UltrasonicSensor(SensorPort.S2));
					test.start();
					bt = true;
					analyserController = new AnalyserController(pilot,adjustIndex,test);
//					panic = new Panic(pilot,new UltrasonicSensor(SensorPort.S2), test);
				}
				
				LCD.drawString("Start in 2", 0,0); Thread.sleep(1000); LCD.drawString("Start in 1", 0,1); Thread.sleep(1000);	

//		analyser.TestBarcodeAnalyser();
		analyseTrack();
	}

	private static void analyseTrack() {	
		boolean finish = false;
		while (!finish) {
			if(bt){
				if(test.isPanic())
				{
					try {
						panic.action();
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
					}
					test.setPanic(false);
				}
			}

			if (track.completeTrack())
			{
				finish = true;
				break;
			}
			int panel = analyserController.AnalysePanel();
			track.update(panel);

			
			
			if (bluetooth)
			{
				test.sendPanel(panel);
			}
			
			

			
			if(bt)
			{
				test.sendPanel(panel); // terug aanzetten!!
			}

			System.out.println("Main: " + panel);

			/*
			 * 1 = straight 6 = right turn 3= left turn
			 */

		}
		LCD.clear();
		for (int tracknr : track.getMap()) {
			System.out.println(tracknr);
		}
		LCD.clear();
		
		pilot.travel(80);
		autoPilot = new AutoPilot(track.getMap(), pilot, adjustIndex);
		autoPilot.travel();
		
	}

	//not used anymore
	private static void loadCalibrationData() {		
		LCD.drawString("Use Calibration data?", 0, 0);
		int b = Button.waitForPress();
		boolean load = false;
		try {
			if (b == 1) {
				File f = new File("calib.dat");
				if (!f.exists())
					throw new FileNotFoundException();
				FileInputStream fis = new FileInputStream(f);
				long bits = 0;
				byte[] bytes = new byte[8];
				fis.read(bytes, 0, 8);
				for (int i = 0; i < 8; i++) {
					bits = (bits << 8) + (long) (bytes[i]);
				}
				circ = Double.longBitsToDouble(bits);
				bits = 0;
				fis.read(bytes, 0, 8);
				for (int i = 0; i < 8; i++) {
					bits = (bits << 8) | bytes[i];
				}
				axis = Double.longBitsToDouble(bits);
				bits = 0L;
				byte byt = 0;
				for (int i = 0; i < 8; i++) {
					byt = (byte) (fis.read());
					bits = (bits << 8) + (long) (byt);
				}
				circ = Double.longBitsToDouble(bits);
				bits = 0L;
				for (int i = 0; i < 8; i++) {
					byt = (byte) (fis.read());
					bits = (bits << 8) + (long) (byt);
				}
				axis = Double.longBitsToDouble(bits);
				fis.close();
				LCD.clearDisplay();
//				LCD.drawString("Lo {
//			case 0: pilot.travel(80);
//aded values:", 0, 0);
				LCD.drawString("Circ:" + circ, 0, 1);
				LCD.drawString("Axis:" + axis, 0, 2);
				Button.waitForPress();
				load = true;
			}
		} catch (Exception e) {
			LCD.drawString("Load error.     ", 0, 0, true);
			Button.waitForPress();
		}
		if (!load) {
			circ = DEF_CIRC;
			axis = DEF_AXIS;
		}
	}

	public static void calibrate() {
		pilot = new TachoPilot((float) DEF_CIRC, (float) DEF_AXIS, Motor.B,
				Motor.C);
//		LCD.drawString("Wheel circ.", 0, 0, true);
//		while (true) {
//			pilot.travel(300);
//
//			double distance = 0.0;
//			double increment = 0.1;
//			LCD.drawString("Increment: " + increment + "cm    ", 0, 2);
//			LCD.drawString("Devation: " + distance + "cm        ", 0, 3);
//
//			Buttonloop: {
//				while (true) {
//					int button = Button.waitForPress();
//					switch (button) {
//					case 1:
//						break Buttonloop;
//					case 2:
//						distance -= increment;
//						break;
//					case 4:
//						distance += increment;
//						break;
//					case 8:
//						if (increment < 1)
//							increment = 1.0;
//						else if (increment < 10)
//							increment = 10.0;
//						else
//							increment = 0.1;
//					}
//					LCD.drawString("Increment: " + Double.toString(increment)
//							+ "cm   ", 0, 2);
//					LCD.drawString("Deviation: " + Double.toString(distance)
//							+ "cm         ", 0, 3);
//				}
//			}
//
//			circ = (300.0 + (distance)) / (300.0 / (Main.circ));
//			LCD.drawString("New: " + circ + "         ", 0, 2);
//			pilot = new TachoPilot((float) circ, (float) Main.axis, Motor.B,
//					Motor.C);
//			int b = Button.waitForPress();
//			if (b == Button.ID_ESCAPE)
//				break;
//		}

		LCD.clearDisplay();
		LCD.drawString("Track width", 0, 0, true);
		while (true) {
			pilot.rotate(-3600);

			double angle = 0;
			double increment = 0.1;
			LCD.drawString("Increment: " + increment + "    ", 0, 2);
			LCD.drawString("Devation: " + angle + "        ", 0, 3);

			Buttonloop: {
				while (true) {
					int button = Button.waitForPress();
					switch (button) {
					case 1:
						break Buttonloop;
					case 2:
						angle -= increment;
						break;
					case 4:
						angle += increment;
						break;
					case 8:
						if (increment < 1)
							increment = 1.0;
						else if (increment < 10)
							increment = 10.0;
						else
							increment = 0.1;
					}
					LCD.drawString("Increment: " + Double.toString(increment)
							+ "   ", 0, 2);
					LCD.drawString("Deviation: " + Double.toString(angle)
							+ "         ", 0, 3);
				}
			}

			Main.axis = (Main.axis * 3600.0) / (3600.0 + angle);
			LCD.drawString("New: " + axis + "         ", 0, 2);
			pilot = new TachoPilot((float) Main.circ, (float) axis, Motor.B,
					Motor.C);
			int b = Button.waitForPress();
			if (b == 8)
				break;
		}

		LCD.clearDisplay();
		LCD.drawString("Save calibration?", 0, 0, true);
		int b = Button.waitForPress();
		try {
			if (b == 1) {
				File f = new File("calib.dat");
				if (f.exists())
					f.delete();
				if (!f.createNewFile())
					throw new IOException();
				FileOutputStream fos = new FileOutputStream(f);
				long bits = Double.doubleToLongBits(circ);
				byte[] bytes = new byte[8];
				for (int i = 0; i < 8; i++) {
					// bytes[i] = (byte)((bits>>>((7-i)*8))&0xff);
					bytes[7 - i] = (byte) (bits & 0xff);
					bits = bits >>> 8;
				}
				for (int i = 0; i < 8; i++) {
					fos.write(bytes[i]);
				}
				bits = Double.doubleToLongBits(axis);
				for (int i = 0; i < 8; i++) {
					// bytes[i] = (byte)((bits>>>((7-i)*8))&0xff);
					bytes[7 - i] = (byte) (bits & 0xff);
					bits = bits >>> 8;
				}
				for (int i = 0; i < 8; i++) {
					fos.write(bytes[i]);
				}
				fos.close();
				LCD.drawString("Saved.       ", 0, 0, true);
			}
		} catch (Exception e) {
			LCD.drawString("Save error.     ", 0, 0, true);
		}
		Button.waitForPress();
	}
	
	public static void testStayInMiddle(){
		List<Integer> list = new ArrayList<Integer>();
		list.add(0);
		while(true){
			AutoPilot autoPilot = new AutoPilot(list, pilot, adjustIndex);
			autoPilot.travel();
			Button.waitForPress();
		}
		
	}
}
